#ifndef PX4_FLIGHT_HANDLER_H
#define PX4_FLIGHT_HANDLER_H

#include <iostream>
#include <fstream>
#include <chrono>
#include <algorithm>
#include <unistd.h>
#include <sys/stat.h>
#include <stdarg.h>
#include <string.h>
#include <map>
#include <mutex>
#include <vector>
#include <string>
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <sys/file.h>
#include <sys/types.h>
#include <sys/statfs.h>
#include <sys/stat.h>
#include <unistd.h>
#include <fcntl.h>
#include <pthread.h>
#include <sys/select.h>
#include <sys/time.h>
#include <sys/types.h>
#include <signal.h>
#include <termios.h>
#include <errno.h>
#include <string.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <sys/wait.h>
#include <arpa/inet.h>
#include <assert.h>
#include <stdint.h>
#include <sys/ioctl.h>
#include <termios.h>
#include <math.h>
#include <thread>
#include <sys/types.h>
#include <dirent.h>

//mavsdk
#include <mavsdk.h>
#include <action/action.h>
#include <offboard/offboard.h>
#include <telemetry/telemetry.h>
#include <mission/mission.h>

#include <mavlink_passthrough/mavlink_passthrough.h>


int64_t getTimeMillis();

void sleepMs(uint32_t timeMs);

double wrap360(const double angle);

double wrap180(const double angle);

class px4FlightHandler
{
public:
	px4FlightHandler();

	~px4FlightHandler();

	bool startUpSystemUdp(const std::string& local_bind_ip, const int local_port);

	bool startUpSystemUart(const std::string& dev_path, const int baudrate);

	void getMavsdkHandler();

	void logInit();

	//log
	bool getLogDir();

	bool createLogFile();

	bool writeLogFile(const char * format, ...);

	void recodeCpuTemperature();

	void fflushLogFile();


	//get flight data
	mavsdk::Telemetry::FlightMode getCurrentFlightMode();

	mavsdk::Telemetry::LandedState getCurrentFlightStatus();

	mavsdk::Telemetry::EulerAngle getCurrentFlightAttitude();

	//control flight
	bool armFlight();

	bool disarmFlight();

	bool takeoffFlight();

	bool takeoffFlight(float relative_altitude_m);

	bool landFlight();

	bool rtlFlight();

	bool guidedFlight(double latitude_deg, double longitude_deg, float altitude_amsl_m, float yaw_deg);

	bool flightIsInAir();

	bool offboardIsActive();

	//offboard attitude control
	bool startOffboardAttitudeRateControl();

	void setAttitudeRate(float roll_deg_s, float pitch_deg_s, float yaw_deg_s, float thrust_value);

	bool stopOffboardAttitudeRateControl();

	bool startOffboardAttitudeControl();

	void setAttitude(float roll_deg, float pitch_deg, float yaw_deg, float thrust_value);

	bool stopOffboardAttitudeControl();

	//offboard position control
	bool startOffboardPositionControl();

	void setPositionNed(float north_m, float east_m, float down_m, float yaw_deg);

	bool stopOffboardPositionControl();

	//offboard velocity contol
	bool startOffboardVelocityNedControl();

	void setVelocityNed(float north_m_s, float east_m_s, float down_m_s, float yaw_deg);

	bool stopOffboardVelocityNedControl();

	bool startOffboardVelocityBodyControl();

	void setVelocityBody(float forward_m_s, float right_m_s, float down_m_s, float yawspeed_deg_s);

	bool stopOffboardVelocityBodyControl();

private:

	//mavsdk
	mavsdk::Mavsdk m_mavsdk;
	std::shared_ptr<mavsdk::Action> m_action;
	std::shared_ptr<mavsdk::Telemetry> m_telemetry;
	std::shared_ptr<mavsdk::Offboard> m_offboard;
	std::shared_ptr<mavsdk::Mission> m_mission;

	//log file
	FILE *m_logFp;
	bool m_getLogDirFlag;
	uint32_t m_lastLogNum;
};


extern px4FlightHandler flightHandler;

#endif //PX4_FLIGHT_HANDLER_H
